// Copyright 2018 Evandro Luis Copercini
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "sdkconfig.h"
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"


#if defined(CONFIG_BT_ENABLED) && defined(CONFIG_BLUEDROID_ENABLED)

#ifdef ARDUINO_ARCH_ESP32
  #include "esp32-hal-log.h"
#endif

#include "BluetoothSerial.h"
#include "BTAdvertisedDevice.h"

#include "esp_bt.h"
#include "esp_bt_main.h"
#include "esp_gap_bt_api.h"
#include "esp_bt_device.h"
#include "esp_spp_api.h"
#include <esp_log.h>

#include "esp32-hal-log.h"

const char * _spp_server_name = "ESP32SPP";

#define RX_QUEUE_SIZE 512
#define TX_QUEUE_SIZE 32
#define SPP_TX_QUEUE_TIMEOUT 1000
#define SPP_TX_DONE_TIMEOUT 1000
#define SPP_CONGESTED_TIMEOUT 1000

static uint32_t _spp_client = 0;
static QueueHandle_t _spp_rx_queue = NULL;
static QueueHandle_t _spp_tx_queue = NULL;
static SemaphoreHandle_t _spp_tx_done = NULL;
static TaskHandle_t _spp_task_handle = NULL;
static EventGroupHandle_t _spp_event_group = NULL;
static EventGroupHandle_t _bt_event_group = NULL;
static boolean secondConnectionAttempt;
static esp_spp_cb_t * custom_spp_callback = NULL;
static BluetoothSerialDataCb custom_data_callback = NULL;
static esp_bd_addr_t current_bd_addr;
static ConfirmRequestCb confirm_request_callback = NULL;
static KeyRequestCb key_request_callback = NULL;
static AuthCompleteCb auth_complete_callback = NULL;
static bool _rmt_name_valid = false;
static uint8_t _rmt_name[ESP_BT_GAP_MAX_BDNAME_LEN + 1] = {0};

#define INQ_LEN 0x10
#define INQ_NUM_RSPS 20
#define READY_TIMEOUT (10 * 1000)
#define SCAN_TIMEOUT (INQ_LEN * 2 * 1000)
static esp_bd_addr_t _peer_bd_addr;
static char _remote_name[ESP_BT_GAP_MAX_BDNAME_LEN + 1];
static bool _isRemoteAddressSet;
static bool _isMaster;
#ifdef CONFIG_BT_SSP_ENABLED
  static bool _enableSSP;
  static bool _IO_CAP_INPUT;
  static bool _IO_CAP_OUTPUT;
#endif
esp_bt_pin_code_t _pin_code = {0};
uint8_t _pin_code_len = 0; // Number of valid Bytes in the esp_bt_pin_code_t array
static esp_spp_sec_t _sec_mask;
static esp_spp_role_t _role;
// start connect on ESP_SPP_DISCOVERY_COMP_EVT or save entry for getChannels
static bool _doConnect;
static std::map <int, std::string> sdpRecords;

static BTScanResultsSet scanResults;
static BTAdvertisedDeviceCb advertisedDeviceCb = nullptr;

// _spp_event_group
#define SPP_RUNNING     0x01
#define SPP_CONNECTED   0x02
#define SPP_CONGESTED   0x04
// true until OPEN successful, changes to false on CLOSE
#define SPP_DISCONNECTED 0x08
// true until connect(), changes to true on CLOSE
#define SPP_CLOSED      0x10

// _bt_event_group
#define BT_DISCOVERY_RUNNING    0x01
#define BT_DISCOVERY_COMPLETED  0x02

#define BT_SDP_RUNNING          0x04
#define BT_SDP_COMPLETED        0x08

typedef struct {
        size_t len;
        uint8_t data[];
} spp_packet_t;

#if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
static char *bda2str(esp_bd_addr_t bda, char *str, size_t size)
{
  if (bda == NULL || str == NULL || size < 18) {
    return NULL;
  }

  uint8_t *p = bda;
  snprintf(str, size, "%02x:%02x:%02x:%02x:%02x:%02x",
          p[0], p[1], p[2], p[3], p[4], p[5]);
  return str;
}
#endif

static bool get_name_from_eir(uint8_t *eir, char *bdname, uint8_t *bdname_len)
{
    if (!eir || !bdname || !bdname_len) {
        return false;
    }

    uint8_t *rmt_bdname, rmt_bdname_len;
    *bdname = *bdname_len = rmt_bdname_len = 0;

    rmt_bdname = esp_bt_gap_resolve_eir_data(eir, ESP_BT_EIR_TYPE_CMPL_LOCAL_NAME, &rmt_bdname_len);
    if (!rmt_bdname) {
        rmt_bdname = esp_bt_gap_resolve_eir_data(eir, ESP_BT_EIR_TYPE_SHORT_LOCAL_NAME, &rmt_bdname_len);
    }
    if (rmt_bdname) {
        rmt_bdname_len = rmt_bdname_len > ESP_BT_GAP_MAX_BDNAME_LEN ? ESP_BT_GAP_MAX_BDNAME_LEN : rmt_bdname_len;
        memcpy(bdname, rmt_bdname, rmt_bdname_len);
        bdname[rmt_bdname_len] = 0;
        *bdname_len = rmt_bdname_len;
        return true;
    }
    return false;
}

static esp_err_t _spp_queue_packet(uint8_t *data, size_t len){
    if(!data || !len){
        log_w("No data provided");
        return ESP_OK;
    }
    spp_packet_t * packet = (spp_packet_t*)malloc(sizeof(spp_packet_t) + len);
    if(!packet){
        log_e("SPP TX Packet Malloc Failed!");
        return ESP_FAIL;
    }
    packet->len = len;
    memcpy(packet->data, data, len);
    if (!_spp_tx_queue || xQueueSend(_spp_tx_queue, &packet, SPP_TX_QUEUE_TIMEOUT) != pdPASS) {
        log_e("SPP TX Queue Send Failed!");
        free(packet);
        return ESP_FAIL;
    }
    return ESP_OK;
}

const uint16_t SPP_TX_MAX = 330;
static uint8_t _spp_tx_buffer[SPP_TX_MAX];
static uint16_t _spp_tx_buffer_len = 0;

static bool _spp_send_buffer(){
    if((xEventGroupWaitBits(_spp_event_group, SPP_CONGESTED, pdFALSE, pdTRUE, SPP_CONGESTED_TIMEOUT) & SPP_CONGESTED) != 0){
        if(!_spp_client){
            log_v("SPP Client Gone!");
            return false;
        }
        log_v("SPP Write %u", _spp_tx_buffer_len);
        esp_err_t err = esp_spp_write(_spp_client, _spp_tx_buffer_len, _spp_tx_buffer);
        if(err != ESP_OK){
            log_e("SPP Write Failed! [0x%X]", err);
            return false;
        }
        _spp_tx_buffer_len = 0;
        if(xSemaphoreTake(_spp_tx_done, SPP_TX_DONE_TIMEOUT) != pdTRUE){
            log_e("SPP Ack Failed!");
            return false;
        }
        return true;
    }
    log_e("SPP Write Congested!");
    return false;
}

static void _spp_tx_task(void * arg){
    spp_packet_t *packet = NULL;
    size_t len = 0, to_send = 0;
    uint8_t * data = NULL;
    for (;;) {
        if(_spp_tx_queue && xQueueReceive(_spp_tx_queue, &packet, portMAX_DELAY) == pdTRUE && packet){
            if(packet->len <= (SPP_TX_MAX - _spp_tx_buffer_len)){
                memcpy(_spp_tx_buffer+_spp_tx_buffer_len, packet->data, packet->len);
                _spp_tx_buffer_len+=packet->len;
                free(packet);
                packet = NULL;
                if(SPP_TX_MAX == _spp_tx_buffer_len || uxQueueMessagesWaiting(_spp_tx_queue) == 0){
                    _spp_send_buffer();
                }
            } else {
                len = packet->len;
                data = packet->data;
                to_send = SPP_TX_MAX - _spp_tx_buffer_len;
                memcpy(_spp_tx_buffer+_spp_tx_buffer_len, data, to_send);
                _spp_tx_buffer_len = SPP_TX_MAX;
                data += to_send;
                len -= to_send;
                if(!_spp_send_buffer()){
                    len = 0;
                }
                while(len >= SPP_TX_MAX){
                    memcpy(_spp_tx_buffer, data, SPP_TX_MAX);
                    _spp_tx_buffer_len = SPP_TX_MAX;
                    data += SPP_TX_MAX;
                    len -= SPP_TX_MAX;
                    if(!_spp_send_buffer()){
                        len = 0;
                        break;
                    }
                }
                if(len){
                    memcpy(_spp_tx_buffer, data, len);
                    _spp_tx_buffer_len += len;
                    if(uxQueueMessagesWaiting(_spp_tx_queue) == 0){
                        _spp_send_buffer();
                    }
                }
                free(packet);
                packet = NULL;
            }
        } else {
            log_e("Something went horribly wrong");
        }
    }
    vTaskDelete(NULL);
    _spp_task_handle = NULL;
}

static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
{
    switch (event)
    {
    case ESP_SPP_INIT_EVT: // Enum 0 - When SPP is initialized
        log_i("ESP_SPP_INIT_EVT");
#ifdef ESP_IDF_VERSION_MAJOR
        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
#else
        esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
#endif
        log_i("ESP_SPP_INIT_EVT: %s: start", _isMaster ? "master" : "slave");
        esp_spp_start_srv(ESP_SPP_SEC_NONE, _isMaster ? ESP_SPP_ROLE_MASTER : ESP_SPP_ROLE_SLAVE, 0, _spp_server_name);
        xEventGroupSetBits(_spp_event_group, SPP_RUNNING);
        break;

    case ESP_SPP_UNINIT_EVT: // Enum 1 - When SPP is deinitialized
        log_i("ESP_SPP_UNINIT_EVT: SPP is deinitialized");
        break;

    case ESP_SPP_DISCOVERY_COMP_EVT: // Enum 8 - When SDP discovery complete
        log_i("ESP_SPP_DISCOVERY_COMP_EVT num=%d", param->disc_comp.scn_num);
        if (param->disc_comp.status == ESP_SPP_SUCCESS) {
            for(int i=0; i < param->disc_comp.scn_num; i++) {
                log_d("ESP_SPP_DISCOVERY_COMP_EVT: spp [%d] channel: %d service name:%s", i, param->disc_comp.scn[i], param->disc_comp.service_name[0]);
            }
            if(_doConnect) {
                if(param->disc_comp.scn_num > 0) {
#if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
                    char bda_str[18];
                    log_i("ESP_SPP_DISCOVERY_COMP_EVT: spp connect to remote %s channel %d",
                        bda2str(_peer_bd_addr, bda_str, sizeof(bda_str)),
                        param->disc_comp.scn[0]);
#endif
                    xEventGroupClearBits(_spp_event_group, SPP_CLOSED);
                    if(esp_spp_connect(_sec_mask, _role, param->disc_comp.scn[0], _peer_bd_addr) != ESP_OK) {
                        log_e("ESP_SPP_DISCOVERY_COMP_EVT connect failed");
                        xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
                    }
                } else {
                    log_e("ESP_SPP_DISCOVERY_COMP_EVT remote doesn't offer an SPP channel");
                    xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
                }
            } else {
                for(int i=0; i < param->disc_comp.scn_num; i++) {
                    sdpRecords[param->disc_comp.scn[i]] = param->disc_comp.service_name[0];
                }
            }
        } else {
            log_e("ESP_SPP_DISCOVERY_COMP_EVT failed!, status:%d", param->disc_comp.status);
        }
        xEventGroupSetBits(_bt_event_group, BT_SDP_COMPLETED);
        break;

    case ESP_SPP_OPEN_EVT: // Enum 26 - When SPP Client connection open
        log_i("ESP_SPP_OPEN_EVT");
        if (!_spp_client){
                _spp_client = param->open.handle;
        } else {
            secondConnectionAttempt = true;
            esp_spp_disconnect(param->open.handle);
        }
        xEventGroupClearBits(_spp_event_group, SPP_DISCONNECTED);
        xEventGroupSetBits(_spp_event_group, SPP_CONNECTED);
        xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
        break;

    case ESP_SPP_CLOSE_EVT: // Enum 27 - When SPP connection closed
        if ((param->close.async == false && param->close.status == ESP_SPP_SUCCESS) || param->close.async) {
            log_i("ESP_SPP_CLOSE_EVT status:%d handle:%d close_by_remote:%d attempt %u", param->close.status,
                 param->close.handle, param->close.async, secondConnectionAttempt);
            if(secondConnectionAttempt) {
                secondConnectionAttempt = false;
            } else {
                _spp_client = 0;
                xEventGroupSetBits(_spp_event_group, SPP_DISCONNECTED);
                xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
                xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
                xEventGroupClearBits(_spp_event_group, SPP_CONNECTED);
            }
        } else {
            log_e("ESP_SPP_CLOSE_EVT failed!, status:%d", param->close.status);
        }
        break;

    case ESP_SPP_START_EVT: // Enum 28 - When SPP server started
        log_i("ESP_SPP_START_EVT");
        break;

    case ESP_SPP_CL_INIT_EVT: // Enum 29 - When SPP client initiated a connection
        if (param->cl_init.status == ESP_SPP_SUCCESS) {
            log_i("ESP_SPP_CL_INIT_EVT handle:%d sec_id:%d", param->cl_init.handle, param->cl_init.sec_id);
        } else {
            log_i("ESP_SPP_CL_INIT_EVT status:%d", param->cl_init.status);
        }
        break;

    case ESP_SPP_DATA_IND_EVT: // Enum 30 - When SPP connection received data, only for ESP_SPP_MODE_CB
        log_v("ESP_SPP_DATA_IND_EVT len=%d handle=%d", param->data_ind.len, param->data_ind.handle);
        //esp_log_buffer_hex("",param->data_ind.data,param->data_ind.len); //for low level debug
        //ets_printf("r:%u\n", param->data_ind.len);

        if(custom_data_callback){
            custom_data_callback(param->data_ind.data, param->data_ind.len);
        } else if (_spp_rx_queue != NULL){
            for (int i = 0; i < param->data_ind.len; i++){
                if(xQueueSend(_spp_rx_queue, param->data_ind.data + i, (TickType_t)0) != pdTRUE){
                    log_e("RX Full! Discarding %u bytes", param->data_ind.len - i);
                    break;
                }
            }
        }
        break;

    case ESP_SPP_CONG_EVT: // Enum 31 - When SPP connection congestion status changed, only for ESP_SPP_MODE_CB
        if(param->cong.cong){
            xEventGroupClearBits(_spp_event_group, SPP_CONGESTED);
        } else {
            xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
        }
        log_v("ESP_SPP_CONG_EVT: %s", param->cong.cong?"CONGESTED":"FREE");
        break;

    case ESP_SPP_WRITE_EVT: // Enum 33 - When SPP write operation completes, only for ESP_SPP_MODE_CB
        if (param->write.status == ESP_SPP_SUCCESS) {
            if(param->write.cong){
                xEventGroupClearBits(_spp_event_group, SPP_CONGESTED);
            }
            log_v("ESP_SPP_WRITE_EVT: %u %s", param->write.len, param->write.cong?"CONGESTED":"");
        } else {
            log_e("ESP_SPP_WRITE_EVT failed!, status:%d", param->write.status);
        }
        xSemaphoreGive(_spp_tx_done);//we can try to send another packet
        break;

    case ESP_SPP_SRV_OPEN_EVT: // Enum 34 - When SPP Server connection open
        if (param->srv_open.status == ESP_SPP_SUCCESS) {
            log_i("ESP_SPP_SRV_OPEN_EVT: %u", _spp_client);
            if (!_spp_client){
                _spp_client = param->srv_open.handle;
                _spp_tx_buffer_len = 0;
            } else {
                secondConnectionAttempt = true;
                esp_spp_disconnect(param->srv_open.handle);
            }
            xEventGroupClearBits(_spp_event_group, SPP_DISCONNECTED);
            xEventGroupSetBits(_spp_event_group, SPP_CONNECTED);
        } else {
            log_e("ESP_SPP_SRV_OPEN_EVT Failed!, status:%d", param->srv_open.status);
        }
        break;

    case ESP_SPP_SRV_STOP_EVT: // Enum 35 - When SPP server stopped
        log_i("ESP_SPP_SRV_STOP_EVT");
        break;

    case ESP_SPP_VFS_REGISTER_EVT: // Enum 36 - When SPP VFS register
        log_i("ESP_SPP_VFS_REGISTER_EVT");
        break;

    case ESP_SPP_VFS_UNREGISTER_EVT: // Enum 37 - When SPP VFS unregister
        log_i("ESP_SPP_VFS_UNREGISTER_EVT");
        break;

    default:
        log_i("ESP_SPP_* event #%d unhandled", event);
        break;
    }
    if(custom_spp_callback)(*custom_spp_callback)(event, param);
}

void BluetoothSerial::onData(BluetoothSerialDataCb cb){
    custom_data_callback = cb;
}


static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param)
{
    switch(event){
        case ESP_BT_GAP_DISC_RES_EVT: { // Enum 0 - Device discovery result event
            log_i("ESP_BT_GAP_DISC_RES_EVT properties=%d", param->disc_res.num_prop);
#if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
            char bda_str[18];
            log_i("Scanned device: %s", bda2str(param->disc_res.bda, bda_str, 18));
#endif
            BTAdvertisedDeviceSet advertisedDevice;
            uint8_t peer_bdname_len = 0;
            char peer_bdname[ESP_BT_GAP_MAX_BDNAME_LEN + 1];
            for (int i = 0; i < param->disc_res.num_prop; i++) {
                switch(param->disc_res.prop[i].type) {
                    case ESP_BT_GAP_DEV_PROP_BDNAME: // Enum 1 - Bluetooth device name, value type is int8_t []
                        peer_bdname_len = param->disc_res.prop[i].len;
                        memcpy(peer_bdname, param->disc_res.prop[i].val, peer_bdname_len);
                        peer_bdname_len--; // len includes 0 terminator
                        log_v("ESP_BT_GAP_DISC_RES_EVT : BDNAME :  %s : %d", peer_bdname, peer_bdname_len);
                        if (strlen(_remote_name) == peer_bdname_len
                            && strncmp(peer_bdname, _remote_name, peer_bdname_len) == 0) {
                            log_i("ESP_BT_GAP_DISC_RES_EVT : SPP_START_DISCOVERY_BDNAME : %s", peer_bdname);
                            _isRemoteAddressSet = true;
                            memcpy(_peer_bd_addr, param->disc_res.bda, ESP_BD_ADDR_LEN);
                            esp_bt_gap_cancel_discovery();
                            esp_spp_start_discovery(_peer_bd_addr);
                        }
                        break;

                    case ESP_BT_GAP_DEV_PROP_COD: // Enum 2 - Class of Device, value type is uint32_t
                        if (param->disc_res.prop[i].len <= sizeof(int)) {
                            uint32_t cod = 0;
                            memcpy(&cod, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
                            advertisedDevice.setCOD(cod);
                            log_d("ESP_BT_GAP_DEV_PROP_COD 0x%x", cod);
                        } else {
                            log_d("ESP_BT_GAP_DEV_PROP_COD invalid COD: Value size larger than integer");
                        }
                        break;

                    case ESP_BT_GAP_DEV_PROP_RSSI: // Enum 3 - Received Signal strength Indication, value type is int8_t, ranging from -128 to 127
                        if (param->disc_res.prop[i].len <= sizeof(int)) {
                            uint8_t rssi = 0;
                            memcpy(&rssi, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
                            log_d("ESP_BT_GAP_DEV_PROP_RSSI %d", rssi);
                            advertisedDevice.setRSSI(rssi);
                        } else {
                            log_d("ESP_BT_GAP_DEV_PROP_RSSI invalid RSSI: Value size larger than integer");
                        }
                        break;

                    case ESP_BT_GAP_DEV_PROP_EIR: // Enum 4 - Extended Inquiry Response, value type is uint8_t []
                        if (get_name_from_eir((uint8_t*)param->disc_res.prop[i].val, peer_bdname, &peer_bdname_len)) {
                            log_i("ESP_BT_GAP_DISC_RES_EVT : EIR : %s : %d", peer_bdname, peer_bdname_len);
                            if (strlen(_remote_name) == peer_bdname_len
                                && strncmp(peer_bdname, _remote_name, peer_bdname_len) == 0) {
                                log_v("ESP_BT_GAP_DISC_RES_EVT : SPP_START_DISCOVERY_EIR : %s", peer_bdname, peer_bdname_len);
                                _isRemoteAddressSet = true;
                                memcpy(_peer_bd_addr, param->disc_res.bda, ESP_BD_ADDR_LEN);
                                esp_bt_gap_cancel_discovery();
                                esp_spp_start_discovery(_peer_bd_addr);
                            }
                        }
                        break;

                    default:
                        log_i("ESP_BT_GAP_DISC_RES_EVT unknown property [%d]:type:%d", i, param->disc_res.prop[i].type);
                        break;
                }
                if (_isRemoteAddressSet)
                    break;
            }
            if (peer_bdname_len)
                advertisedDevice.setName(peer_bdname);
            esp_bd_addr_t addr;
            memcpy(addr, param->disc_res.bda, ESP_BD_ADDR_LEN);
            advertisedDevice.setAddress(BTAddress(addr));
            if (scanResults.add(advertisedDevice) && advertisedDeviceCb)
                advertisedDeviceCb(&advertisedDevice);
        }
        break;

        case ESP_BT_GAP_DISC_STATE_CHANGED_EVT: // Enum 1 - Discovery state changed event
            if (param->disc_st_chg.state == ESP_BT_GAP_DISCOVERY_STOPPED) {
                log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT stopped");
                xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_RUNNING);
                xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
            } else { // ESP_BT_GAP_DISCOVERY_STARTED
                log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT started");
                xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
                xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_RUNNING);
            }
            break;

        case ESP_BT_GAP_RMT_SRVCS_EVT: // Enum 2 - Get remote services event
            log_i( "ESP_BT_GAP_RMT_SRVCS_EVT: status = %d, num_uuids = %d", param->rmt_srvcs.stat, param->rmt_srvcs.num_uuids);
            break;

        case ESP_BT_GAP_RMT_SRVC_REC_EVT: // Enum 3 - Get remote service record event
            log_i("ESP_BT_GAP_RMT_SRVC_REC_EVT: status = %d", param->rmt_srvc_rec.stat);
            break;

        case ESP_BT_GAP_AUTH_CMPL_EVT: // Enum 4 - Authentication complete event
            if (param->auth_cmpl.stat == ESP_BT_STATUS_SUCCESS) {
                log_v("authentication success: %s", param->auth_cmpl.device_name);
                if (auth_complete_callback) {
                    auth_complete_callback(true);
                }
            } else {
                log_e("authentication failed, status:%d", param->auth_cmpl.stat);
                if (auth_complete_callback) {
                    auth_complete_callback(false);
                }
            }
            break;
        case ESP_BT_GAP_PIN_REQ_EVT: // Enum 5 - Legacy Pairing Pin code request
            log_i("ESP_BT_GAP_PIN_REQ_EVT (min_16_digit=%d)", param->pin_req.min_16_digit);
            if (param->pin_req.min_16_digit && _pin_code_len < 16) {
                esp_bt_gap_pin_reply(param->pin_req.bda, false, 0, NULL);
            } else {
                //log_i("Input pin code: \"%s\"=0x%x", _pin_code);
                log_i("Input pin code: \"%.*s\"=0x%x", _pin_code_len, _pin_code, *(int*)_pin_code);
                esp_bt_gap_pin_reply(param->pin_req.bda, true, _pin_code_len, _pin_code);
            }
            break;
#ifdef CONFIG_BT_SSP_ENABLED
        case ESP_BT_GAP_CFM_REQ_EVT: // Enum 6 - Security Simple Pairing User Confirmation request.
            log_i("ESP_BT_GAP_CFM_REQ_EVT Please compare the numeric value: %d", param->cfm_req.num_val);
            if (confirm_request_callback) {
                memcpy(current_bd_addr, param->cfm_req.bda, sizeof(esp_bd_addr_t));
                confirm_request_callback(param->cfm_req.num_val);
            }
            else {
                log_w("ESP_BT_GAP_CFM_REQ_EVT: confirm_request_callback does not exist - refusing pairing");
                esp_bt_gap_ssp_confirm_reply(param->cfm_req.bda, false);
            }
            break;
#endif

        case ESP_BT_GAP_KEY_NOTIF_EVT: // Enum 7 - Security Simple Pairing Passkey Notification
            log_i("ESP_BT_GAP_KEY_NOTIF_EVT passkey:%d", param->key_notif.passkey);
            break;

#ifdef CONFIG_BT_SSP_ENABLED
        case ESP_BT_GAP_KEY_REQ_EVT: // Enum 8 - Security Simple Pairing Passkey request
            log_i("ESP_BT_GAP_KEY_REQ_EVT Please enter passkey!");
            if (key_request_callback) {
                memcpy(current_bd_addr, param->cfm_req.bda, sizeof(esp_bd_addr_t));
                key_request_callback();
            } else {
                log_w("ESP_BT_GAP_KEY_REQ_EVT: key_request_callback does not exist - refuseing pairing");
                esp_bt_gap_ssp_confirm_reply(param->cfm_req.bda, false);
            }
            break;
#endif

        case ESP_BT_GAP_READ_RSSI_DELTA_EVT: // Enum 9 - Read rssi event
            log_i("ESP_BT_GAP_READ_RSSI_DELTA_EVT Read rssi event");
            break;
        case ESP_BT_GAP_CONFIG_EIR_DATA_EVT: // Enum 10 - Config EIR data event
            log_i("ESP_BT_GAP_CONFIG_EIR_DATA_EVT: stat:%d num:%d", param->config_eir_data.stat, param->config_eir_data.eir_type_num);
            break;

        case ESP_BT_GAP_SET_AFH_CHANNELS_EVT: // Enum 11 - Set AFH channels event
            log_i("ESP_BT_GAP_SET_AFH_CHANNELS_EVT Set AFH channels event");
            break;

        case ESP_BT_GAP_READ_REMOTE_NAME_EVT: // Enum 12 - Read Remote Name event
            if (param->read_rmt_name.stat == ESP_BT_STATUS_SUCCESS ) {
                log_i("ESP_BT_GAP_READ_REMOTE_NAME_EVT: %s", param->read_rmt_name.rmt_name);
                memcpy(_rmt_name, param->read_rmt_name.rmt_name, ESP_BT_GAP_MAX_BDNAME_LEN + 1);
                _rmt_name_valid = true;
            } else {
                log_i("ESP_BT_GAP_READ_REMOTE_NAME_EVT: no success stat:%d", param->read_rmt_name.stat);
            }
            break;


        case ESP_BT_GAP_MODE_CHG_EVT: // Enum 13
            log_i("ESP_BT_GAP_MODE_CHG_EVT: mode: %d", param->mode_chg.mode);
            break;

        case ESP_BT_GAP_REMOVE_BOND_DEV_COMPLETE_EVT: // Enum - 14 remove bond device complete event
            log_i("ESP_BT_GAP_REMOVE_BOND_DEV_COMPLETE_EVT remove bond device complete event");
            break;

        case ESP_BT_GAP_QOS_CMPL_EVT: // Enum 15 - QOS complete event
            log_i("ESP_BT_GAP_QOS_CMPL_EVT QOS complete event");
            break;

        case ESP_BT_GAP_ACL_CONN_CMPL_STAT_EVT: // Enum 16 - ACL connection complete status event
            log_i("ESP_BT_GAP_ACL_CONN_CMPL_STAT_EVT ACL connection complete status event");
            break;

        case ESP_BT_GAP_ACL_DISCONN_CMPL_STAT_EVT: // Enum 17 - ACL disconnection complete status event
            log_i("ESP_BT_GAP_ACL_DISCONN_CMPL_STAT_EVT ACL disconnection complete status event: reason %d, handle %d", param->acl_disconn_cmpl_stat.reason, param->acl_disconn_cmpl_stat.handle);
            break;

        default:
            log_i("ESP-BT_GAP_* unknown message: %d", event);
            break;
    }
}

static bool _init_bt(const char *deviceName)
{
    if(!_bt_event_group){
        _bt_event_group = xEventGroupCreate();
        if(!_bt_event_group){
            log_e("BT Event Group Create Failed!");
            return false;
        }
        xEventGroupClearBits(_bt_event_group, 0xFFFFFF);
    }
    if(!_spp_event_group){
        _spp_event_group = xEventGroupCreate();
        if(!_spp_event_group){
            log_e("SPP Event Group Create Failed!");
            return false;
        }
        xEventGroupClearBits(_spp_event_group, 0xFFFFFF);
        xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
        xEventGroupSetBits(_spp_event_group, SPP_DISCONNECTED);
        xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
    }
    if (_spp_rx_queue == NULL){
        _spp_rx_queue = xQueueCreate(RX_QUEUE_SIZE, sizeof(uint8_t)); //initialize the queue
        if (_spp_rx_queue == NULL){
            log_e("RX Queue Create Failed");
            return false;
        }
    }
    if (_spp_tx_queue == NULL){
        _spp_tx_queue = xQueueCreate(TX_QUEUE_SIZE, sizeof(spp_packet_t*)); //initialize the queue
        if (_spp_tx_queue == NULL){
            log_e("TX Queue Create Failed");
            return false;
        }
    }
    if(_spp_tx_done == NULL){
        _spp_tx_done = xSemaphoreCreateBinary();
        if (_spp_tx_done == NULL){
            log_e("TX Semaphore Create Failed");
            return false;
        }
        xSemaphoreTake(_spp_tx_done, 0);
    }

    if(!_spp_task_handle){
        xTaskCreatePinnedToCore(_spp_tx_task, "spp_tx", 4096, NULL, 10, &_spp_task_handle, 0);
        if(!_spp_task_handle){
            log_e("Network Event Task Start Failed!");
            return false;
        }
    }

    if (!btStarted() && !btStart()){
        log_e("initialize controller failed");
        return false;
    }
    
    esp_bluedroid_status_t bt_state = esp_bluedroid_get_status();
    if (bt_state == ESP_BLUEDROID_STATUS_UNINITIALIZED){
        if (esp_bluedroid_init()) {
            log_e("initialize bluedroid failed");
            return false;
        }
    }
    
    if (bt_state != ESP_BLUEDROID_STATUS_ENABLED){
        if (esp_bluedroid_enable()) {
            log_e("enable bluedroid failed");
            return false;
        }
    }

    if (esp_bt_gap_register_callback(esp_bt_gap_cb) != ESP_OK) {
        log_e("gap register failed");
        return false;
    }

    if (esp_spp_register_callback(esp_spp_cb) != ESP_OK){
        log_e("spp register failed");
        return false;
    }

    esp_spp_cfg_t cfg = BT_SPP_DEFAULT_CONFIG();
    cfg.mode = ESP_SPP_MODE_CB;
    if (esp_spp_enhanced_init(&cfg) != ESP_OK){
        log_e("spp init failed");
        return false;
    }

    log_i("device name set");
    esp_bt_dev_set_device_name(deviceName);

#ifdef CONFIG_BT_SSP_ENABLED
    if (_enableSSP) {
        log_i("Simple Secure Pairing");
        esp_bt_sp_param_t param_type = ESP_BT_SP_IOCAP_MODE;
        esp_bt_io_cap_t iocap;
        if(_IO_CAP_INPUT && _IO_CAP_OUTPUT){
            iocap = ESP_BT_IO_CAP_IO; // Display with prompt
        }else if(!_IO_CAP_INPUT && _IO_CAP_OUTPUT){
            iocap = ESP_BT_IO_CAP_OUT; // DisplayOnly
        }else if(_IO_CAP_INPUT && !_IO_CAP_OUTPUT){
            iocap = ESP_BT_IO_CAP_IN; // Input only
        }else if(!_IO_CAP_INPUT && !_IO_CAP_OUTPUT){
            iocap = ESP_BT_IO_CAP_NONE; // No input/output
        }
        esp_bt_gap_set_security_param(param_type, &iocap, sizeof(uint8_t));
    }
#endif

    // the default BTA_DM_COD_LOUDSPEAKER does not work with the macOS BT stack
    esp_bt_cod_t cod;
    cod.major = 0b00001;
    cod.minor = 0b000100;
    cod.service = 0b00000010110;
    if (esp_bt_gap_set_cod(cod, ESP_BT_INIT_COD) != ESP_OK) {
        log_e("set cod failed");
        return false;
    }
    return true;
}

static bool _stop_bt()
{
    if (btStarted()){
        if(_spp_client)
            esp_spp_disconnect(_spp_client);
        esp_spp_deinit();
        esp_bluedroid_disable();
        esp_bluedroid_deinit();
        btStop();
    }
    _spp_client = 0;
    if(_spp_task_handle){
        vTaskDelete(_spp_task_handle);
        _spp_task_handle = NULL;
    }
    if(_spp_event_group){
        vEventGroupDelete(_spp_event_group);
        _spp_event_group = NULL;
    }
    if(_spp_rx_queue){
        vQueueDelete(_spp_rx_queue);
        //ToDo: clear RX queue when in packet mode
        _spp_rx_queue = NULL;
    }
    if(_spp_tx_queue){
        spp_packet_t *packet = NULL;
        while(xQueueReceive(_spp_tx_queue, &packet, 0) == pdTRUE){
            free(packet);
        }
        vQueueDelete(_spp_tx_queue);
        _spp_tx_queue = NULL;
    }
    if (_spp_tx_done) {
        vSemaphoreDelete(_spp_tx_done);
        _spp_tx_done = NULL;
    }
    if (_bt_event_group) {
        vEventGroupDelete(_bt_event_group);
        _bt_event_group = NULL;
    }
    return true;
}

static bool waitForConnect(int timeout) {
    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
    // wait for connected or closed
    EventBits_t rc = xEventGroupWaitBits(_spp_event_group, SPP_CONNECTED | SPP_CLOSED, pdFALSE, pdFALSE, xTicksToWait);
    if((rc & SPP_CONNECTED) != 0)
        return true;
    else if((rc & SPP_CLOSED) != 0) {
        log_d("connection closed!");
        return false;
    }
    log_d("timeout");
    return false;
}

static bool waitForDiscovered(int timeout) {
    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
    return (xEventGroupWaitBits(_spp_event_group, BT_DISCOVERY_COMPLETED, pdFALSE, pdTRUE, xTicksToWait) & BT_DISCOVERY_COMPLETED) != 0;
}

static bool waitForSDPRecord(int timeout) {
    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
    return (xEventGroupWaitBits(_bt_event_group, BT_SDP_COMPLETED, pdFALSE, pdTRUE, xTicksToWait) & BT_SDP_COMPLETED) != 0;
}

/*
 * Serial Bluetooth Arduino
 *
 * */

BluetoothSerial::BluetoothSerial()
{
    local_name = "ESP32"; //default bluetooth name
}

BluetoothSerial::~BluetoothSerial(void)
{
    _stop_bt();
}

/**
 * @Param isMaster set to true if you want to connect to an other device
 */
bool BluetoothSerial::begin(String localName, bool isMaster)
{
    _isMaster = isMaster;
    if (localName.length()){
        local_name = localName;
    }
    return _init_bt(local_name.c_str());
}

int BluetoothSerial::available(void)
{
    if (_spp_rx_queue == NULL){
        return 0;
    }
    return uxQueueMessagesWaiting(_spp_rx_queue);
}

int BluetoothSerial::peek(void)
{
    uint8_t c;
    if (_spp_rx_queue && xQueuePeek(_spp_rx_queue, &c, this->timeoutTicks)){
        return c;
    }
    return -1;
}

bool BluetoothSerial::hasClient(void)
{
    return _spp_client > 0;
}

int BluetoothSerial::read()
{

    uint8_t c = 0;
    if (_spp_rx_queue && xQueueReceive(_spp_rx_queue, &c, this->timeoutTicks)){
        return c;
    }
    return -1;
}

/**
 * Set timeout for read / peek
 */
void BluetoothSerial::setTimeout(int timeoutMS)
{
    Stream::setTimeout(timeoutMS);
    this->timeoutTicks=timeoutMS / portTICK_PERIOD_MS;
}

size_t BluetoothSerial::write(uint8_t c)
{
    return write(&c, 1);
}

size_t BluetoothSerial::write(const uint8_t *buffer, size_t size)
{
    if (!_spp_client){
        return 0;
    }
    return (_spp_queue_packet((uint8_t *)buffer, size) == ESP_OK) ? size : 0;
}

void BluetoothSerial::flush()
{
    if (_spp_tx_queue != NULL){
        while(uxQueueMessagesWaiting(_spp_tx_queue) > 0){
           delay(100);
        }
    }
}

void BluetoothSerial::end()
{
    _stop_bt();
}

#ifdef CONFIG_BT_SSP_ENABLED
void BluetoothSerial::onConfirmRequest(ConfirmRequestCb cb)
{
    confirm_request_callback = cb;
}

void BluetoothSerial::onKeyRequest(KeyRequestCb cb)
{
    key_request_callback = cb;
}

void BluetoothSerial::respondPasskey(uint32_t passkey){
    esp_bt_gap_ssp_passkey_reply(current_bd_addr, true, passkey);
}
#endif

void BluetoothSerial::onAuthComplete(AuthCompleteCb cb)
{
    auth_complete_callback = cb;
}

void BluetoothSerial::confirmReply(boolean confirm)
{
    esp_bt_gap_ssp_confirm_reply(current_bd_addr, confirm);
}


esp_err_t BluetoothSerial::register_callback(esp_spp_cb_t * callback)
{
    custom_spp_callback = callback;
    return ESP_OK;
}

#ifdef CONFIG_BT_SSP_ENABLED
// Enable Simple Secure Pairing (using generated PIN)
// This must be called before calling begin, otherwise has no effect!
void BluetoothSerial::enableSSP() {
    if(isReady(false, READY_TIMEOUT)){
        log_i("Attempted to enable SSP for already initialized driver. Restart to take effect with end() followed by begin()");
        return;
    }
    _enableSSP = true;
    _IO_CAP_INPUT = true;
    _IO_CAP_OUTPUT = true;
}

// Enable Simple Secure Pairing (using generated PIN)
// This must be called before calling begin, otherwise has no effect!
// Behavior description:
// When both Input and Output are false only the other device authenticates pairing without any pin.
// When Output is true and Input is false only the other device authenticates pairing without any pin.
// When both Input and Output are true both devices display randomly generated code and if they match authenticate pairing on both devices
//   - This must be implemented by registering callback via onConfirmRequest() and in this callback request user input and call confirmReply(true); if the authenticated
//      otherwise call `confirmReply(false)` to reject the pairing.
// When Input is true and Output is false User will be required to input the passkey to the ESP32 device to authenticate.
//   - This must be implemented by registering callback via onKeyRequest() and in this callback the entered passkey will be responded via respondPasskey(passkey);
void BluetoothSerial::enableSSP(bool inputCpability, bool outputCapability) {
    log_i("Enabling SSP: input capability=%d; output capability=%d", inputCpability, outputCapability);
    _enableSSP = true;
    _IO_CAP_INPUT = inputCpability;
    _IO_CAP_OUTPUT = outputCapability;
}

// Disable Simple Secure Pairing (using generated PIN)
// This must be called before calling begin, otherwise has no effect!
void BluetoothSerial::disableSSP() {
    _enableSSP = false;
}

#else

bool BluetoothSerial::setPin(const char *pin, uint8_t pin_code_len){
    if(pin_code_len == 0 || pin_code_len > 16){
        log_e("PIN code must be 1-16 Bytes long! Called with length %d", pin_code_len);
        return false;
    }
    _pin_code_len = pin_code_len;
    memcpy(_pin_code, pin, pin_code_len);
    return (esp_bt_gap_set_pin(ESP_BT_PIN_TYPE_FIXED, _pin_code_len, _pin_code) == ESP_OK);
}
#endif

bool BluetoothSerial::connect(String remoteName)
{
    bool retval = false;

    if (!isReady(true, READY_TIMEOUT)) return false;
    if (remoteName && remoteName.length() < 1) {
        log_e("No remote name is provided");
        return false; 
    }
    disconnect();
    _doConnect = true;
    _isRemoteAddressSet = true;
    _sec_mask = ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE;
    _role = ESP_SPP_ROLE_MASTER;
    strncpy(_remote_name, remoteName.c_str(), ESP_BT_GAP_MAX_BDNAME_LEN);
    _remote_name[ESP_BT_GAP_MAX_BDNAME_LEN] = 0;
    log_i("master : remoteName");
    // will first resolve name to address
#ifdef ESP_IDF_VERSION_MAJOR
        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
#else
        esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
#endif
    xEventGroupClearBits(_spp_event_group, SPP_CLOSED);
    if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
        retval = waitForConnect(SCAN_TIMEOUT);
    }
    if (retval == false) {
      _isRemoteAddressSet = false;
    }
    return retval;
}

/**
 * @Param channel: specify channel or 0 for auto-detect
 * @Param sec_mask:
 *           ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE
 *           ESP_SPP_SEC_NONE
 * @Param role:
 *           ESP_SPP_ROLE_MASTER   master can handle up to 7 connections to slaves
 *           ESP_SPP_ROLE_SLAVE    can only have one connection to a master
 */
bool BluetoothSerial::connect(uint8_t remoteAddress[], int channel, esp_spp_sec_t sec_mask, esp_spp_role_t role)
{
    bool retval = false;
    if (!isReady(true, READY_TIMEOUT)) return false;
    if (!remoteAddress) {
        log_e("No remote address is provided");
        return false; 
    }
    disconnect();
    _doConnect = true;
    _remote_name[0] = 0;
    _isRemoteAddressSet = true;
    _sec_mask = sec_mask;
    _role = role;
    memcpy(_peer_bd_addr, remoteAddress, ESP_BD_ADDR_LEN);
    log_i("master : remoteAddress");
    xEventGroupClearBits(_spp_event_group, SPP_CLOSED);
    if (channel > 0) {
#if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
        char bda_str[18];
        log_i("spp connect to remote %s channel %d",
            bda2str(_peer_bd_addr, bda_str, sizeof(bda_str)),
            channel);
#endif
        if(esp_spp_connect(sec_mask, role, channel, _peer_bd_addr) != ESP_OK ) {
            log_e("spp connect failed");
      retval = false;
    } else {
      retval = waitForConnect(READY_TIMEOUT);
      if(retval) {
            log_i("connected");
        } else {
            if(this->isClosed()) {
                log_e("connect failed");
            } else {
                log_e("connect timed out after %dms", READY_TIMEOUT);
            }
        }
    }
  } else if (esp_spp_start_discovery(_peer_bd_addr) == ESP_OK) {
    retval = waitForConnect(READY_TIMEOUT);
  }

  if (!retval) {
    _isRemoteAddressSet = false;
    }
    return retval;
}

bool BluetoothSerial::connect()
{
    if (!isReady(true, READY_TIMEOUT)) return false;
    _doConnect = true;
    if (_isRemoteAddressSet){
        disconnect();
        // use resolved or set address first
        log_i("master : remoteAddress");
        if (esp_spp_start_discovery(_peer_bd_addr) == ESP_OK) {
            return waitForConnect(READY_TIMEOUT);
        }
        return false;
    } else if (_remote_name[0]) {
        disconnect();
        log_i("master : remoteName");
        // will resolve name to address first - it may take a while
#ifdef ESP_IDF_VERSION_MAJOR
        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
#else
        esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
#endif
        if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
            return waitForConnect(SCAN_TIMEOUT);
        }
        return false;
    }
    log_e("Neither Remote name nor address was provided");
    return false;
}

bool BluetoothSerial::disconnect() {
    if (_spp_client) {
        flush();
        log_i("disconnecting");
        if (esp_spp_disconnect(_spp_client) == ESP_OK) {
            TickType_t xTicksToWait = READY_TIMEOUT / portTICK_PERIOD_MS;
            return (xEventGroupWaitBits(_spp_event_group, SPP_DISCONNECTED, pdFALSE, pdTRUE, xTicksToWait) & SPP_DISCONNECTED) != 0;
        }
    }
    return false;
}

bool BluetoothSerial::unpairDevice(uint8_t remoteAddress[]) {
    if (isReady(false, READY_TIMEOUT)) {
        log_i("removing bonded device");
        return (esp_bt_gap_remove_bond_device(remoteAddress) == ESP_OK);
    }
    return false;
}

bool BluetoothSerial::connected(int timeout) {
    return waitForConnect(timeout);
}

/**
 * true if a connection terminated or a connection attempt failed
 */
bool BluetoothSerial::isClosed() {
    return xEventGroupGetBits(_spp_event_group) & SPP_CLOSED;
}

bool BluetoothSerial::isReady(bool checkMaster, int timeout) {
    if (checkMaster && !_isMaster) {
        log_e("Master mode is not active. Call begin(localName, true) to enable Master mode");
        return false;
    }
    if (!btStarted()) {
        log_e("BT is not initialized. Call begin() first");
        return false;
    }
    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
    return (xEventGroupWaitBits(_spp_event_group, SPP_RUNNING, pdFALSE, pdTRUE, xTicksToWait) & SPP_RUNNING) != 0;
}


/**
 * @brief           RemoteName or address are not allowed to be set during discovery
 *                  (otherwise it might connect automatically and stop discovery)
 * @param[in]       timeoutMs can range from MIN_INQ_TIME to MAX_INQ_TIME
 * @return          in case of Error immediately Empty ScanResults.
 */
BTScanResults* BluetoothSerial::discover(int timeoutMs) {
    scanResults.clear();
    if (timeoutMs < MIN_INQ_TIME || timeoutMs > MAX_INQ_TIME){
        log_e("Timeout out of bounds: MIN=%d; MAX=%d; requested=%d", MIN_INQ_TIME, MAX_INQ_TIME, timeoutMs);
        return nullptr;
    }
    int timeout = timeoutMs / INQ_TIME;
    log_i("discover::disconnect");
    disconnect();
    log_i("discovering");
    // will resolve name to address first - it may take a while
    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
    if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK) {
        waitForDiscovered(timeoutMs);
        log_i("gap_cancel_discovery()");
        esp_bt_gap_cancel_discovery();
    }
    return &scanResults;
}

/**
 * @brief           RemoteName or address are not allowed to be set during discovery
 *                  (otherwise it might connect automatically and stop discovery)
 * @param[in]       cb called when a [b]new[/b] device has been discovered
 * @param[in]       timeoutMs can be 0 or range from MIN_INQ_TIME to MAX_INQ_TIME
 *
 * @return          Whether start was successful or problems with params
 */
bool BluetoothSerial::discoverAsync(BTAdvertisedDeviceCb cb, int timeoutMs) {
    scanResults.clear();
    if (strlen(_remote_name) || _isRemoteAddressSet)
        return false;
    int timeout = timeoutMs / INQ_TIME;
    disconnect();
    advertisedDeviceCb = cb;
    log_i("discovering");
    // will resolve name to address first - it may take a while
    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
    if (timeout > 0)
        return esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK;
    else return esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, ESP_BT_GAP_MAX_INQ_LEN, 0) == ESP_OK;
}

/** @brief      Stops the asynchronous discovery and clears the callback */
void BluetoothSerial::discoverAsyncStop() {
    esp_bt_gap_cancel_discovery();
    advertisedDeviceCb = nullptr;
}

/** @brief      Clears scanResult entries */
void BluetoothSerial::discoverClear() {
    scanResults.clear();
}

/** @brief      Can be used while discovering asynchronously
 *              Will be returned also on synchronous discovery.
 *
 * @return      BTScanResults contains several information of found devices
 */
BTScanResults* BluetoothSerial::getScanResults() {
    return &scanResults;
}

BluetoothSerial::operator bool() const
{
    return true;
}

/**
 * SDP scan address
 * esp_spp_start_discovery doesn't tell us the btAddress in the callback, so we have to wait until it's finished
 */
std::map<int, std::string> BluetoothSerial::getChannels(const BTAddress &remoteAddress) {
    if(xEventGroupGetBits(_bt_event_group) & BT_SDP_RUNNING) {
        log_e("getChannels failed - already running");
    }
    xEventGroupSetBits(_bt_event_group, BT_SDP_RUNNING);
    xEventGroupClearBits(_bt_event_group, BT_SDP_COMPLETED);
    _doConnect = false;
    sdpRecords.clear();
    log_d("esp_spp_start_discovery");
    if (esp_spp_start_discovery(*remoteAddress.getNative()) != ESP_OK) {
        log_e("esp_spp_start_discovery failed");
    } else {
        if(! waitForSDPRecord(READY_TIMEOUT)) {
            log_e("getChannels failed timeout");
        }
        log_d("esp_spp_start_discovery wait for BT_SDP_COMPLETED done (%dms)", READY_TIMEOUT);
    }
    log_d("esp_spp_start_discovery done, found %d services", sdpRecords.size());
    xEventGroupClearBits(_bt_event_group, BT_SDP_RUNNING);
    return sdpRecords;
}

/**
 * @brief      Gets the MAC address of local BT device in byte array.
 *
 * @param      mac [out]  The mac
 */
void BluetoothSerial::getBtAddress(uint8_t *mac) {
    const uint8_t *dev_mac = esp_bt_dev_get_address();
    memcpy(mac, dev_mac, ESP_BD_ADDR_LEN);
}
/**
 * @brief      Gets the MAC address of local BT device as BTAddress object.
 *
 * @return     The BTAddress object.
 */
BTAddress BluetoothSerial::getBtAddressObject() {
    uint8_t mac_arr[ESP_BD_ADDR_LEN];
    getBtAddress(mac_arr);
    return BTAddress(mac_arr);
}
/**
 * @brief      Gets the MAC address of local BT device as string.
 *
 * @return     The BT MAC address string.
 */
String BluetoothSerial::getBtAddressString() {
    return getBtAddressObject().toString(true);
}

// Send a request to the remote device defined by the remoteAddress to send back its name.
// The name will be read by background task and stored. It can be later read with radRemoteName()
void BluetoothSerial::requestRemoteName(uint8_t remoteAddress[]){
    if(isReady(false, READY_TIMEOUT)){
        esp_bt_gap_read_remote_name(remoteAddress);
    }
}

// If remote name is valid (was already received) this function will copy the name to the aprameter rmt_name
// The buffer must have size at least ESP_BT_GAP_MAX_BDNAME_LEN + 1
// If the name is valid the function will return true
// If the name is not valid (was not read yet) returns false
bool BluetoothSerial::readRemoteName(char rmt_name[ESP_BT_GAP_MAX_BDNAME_LEN + 1]){
    if(_rmt_name_valid){
        memcpy(rmt_name, _rmt_name, ESP_BT_GAP_MAX_BDNAME_LEN + 1);
        return true;
    }
    return false;
}

// Set validity of remote name before reading name from different device
void BluetoothSerial::invalidateRemoteName(){
    _rmt_name_valid = false;
}

int BluetoothSerial::getNumberOfBondedDevices(){
    return esp_bt_gap_get_bond_device_num();
}

// Accepts the maximum number of devices that can fit in given array dev_list.
// Create you list this way: esp_bd_addr_t dev_list[dev_num];
// Returns number of retrieved devices (on error returns 0)
int BluetoothSerial::getBondedDevices(uint dev_num, esp_bd_addr_t *dev_list){
    // typedef uint8_t esp_bd_addr_t[ESP_BD_ADDR_LEN]
    if(dev_list == NULL){
        log_e("Device list is NULL");
        return 0;
    }
    if(dev_num == 0){
        log_e("Device number must be larger than 0!");
        return 0;
    }
    int _dev_num = dev_num;
    esp_bt_gap_get_bond_device_list(&_dev_num, dev_list);
    return _dev_num;
}

bool BluetoothSerial::deleteBondedDevice(uint8_t *remoteAddress){
    esp_err_t ret = esp_bt_gap_remove_bond_device(remoteAddress);
    if(ret == ESP_OK){
        return true;
    }else{
        return false;
    }
}

void BluetoothSerial::deleteAllBondedDevices(){
    if(!isReady(false, READY_TIMEOUT)){
        log_w("Attempted to drop cache for uninitialized driver. First call begin()");
        return;
    }

    int expected_dev_num = esp_bt_gap_get_bond_device_num();
    if(expected_dev_num == 0){
        log_i("No devices in cache.");
        return;
    } else {
        log_d("Found %d bonded devices", expected_dev_num);
    }
    esp_err_t ret;

    // typedef uint8_t esp_bd_addr_t[ESP_BD_ADDR_LEN] // ESP_BD_ADDR_LEN = 6
    esp_bd_addr_t *dev_list = NULL;
    log_d("Allocate buffer: sizeof(esp_bd_addr_t)=%d * expected_dev_num=%d", sizeof(esp_bd_addr_t), expected_dev_num);
    dev_list = (esp_bd_addr_t*) malloc(sizeof(esp_bd_addr_t) * expected_dev_num);
    if(dev_list == NULL){
        log_e("Could not allocated BT device buffer!");
        return;
    }
    //uint8_t dev_list [20][6];

    int dev_num;
    ret = esp_bt_gap_get_bond_device_list(&dev_num, dev_list);
    log_d("esp_bt_gap_get_bond_device_list ret = %d", ret);
    if(ret == ESP_OK){
        if(dev_num != expected_dev_num){
            log_w("Inconsistent number of bonded devices. Expected %d; returned %d",expected_dev_num, dev_num);
        }
        for(int i=0; i<dev_num; ++i){
            ret = esp_bt_gap_remove_bond_device(dev_list[i]);
            log_d("esp_bt_gap_remove_bond_device ret = %d", ret);
            if(ret == ESP_OK){
                log_d("Removed bonded device #%d", i);
            } else {
                log_w("Failed to removed bonded device #%d", i);
            }
            //btc_storage_remove_bonded_device(dev_list[i]);
        }
        log_d("device num after delete = %d", esp_bt_gap_get_bond_device_num());
    }else{
        log_w("Function esp_bt_gap_get_bond_device_list() returned code %d", ret);
    }
}
#endif // defined(CONFIG_BT_ENABLED) && defined(CONFIG_BLUEDROID_ENABLED)
